/*
 * Engines.h
 *
 *  Created on: Jan 11, 2010
 *      Author: vuk
 */

#ifndef ENGINES_H_
#define ENGINES_H_

#include <opencv/cv.h>
//#include <opencv/highgui.h>
//#include <stdio.h>
//#include <stdlib.h>
//#include <iostream>

#include <SerialStream.h>

using namespace std;
using namespace LibSerial;

class Engines
{
	SerialStream serial;
	int min, max;

	char leftPower, rightPower;
	char messageId;

public:
	Engines(const int minimal=0, const int maximal=32, const char* file="/dev/ttyUSB0")
	:min(minimal),max(maximal)
	{
		messageId=0;

		serial.Open(file);
		serial.SetBaudRate(SerialStreamBuf::BAUD_19200);
		serial.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
		serial.SetParity(SerialStreamBuf::PARITY_NONE);
		serial.SetNumOfStopBits(1);
		serial.SetFlowControl(SerialStreamBuf::FLOW_CONTROL_NONE);
	}

	~Engines()
	{
		serial.Close();
	}

	void CRC16(const char &Data, unsigned short &crc)
	{ // Racuna CRC
		char tmp;	// dovoljan je byte jer uvek testiram b0
		crc ^= Data;
		for (int i = 0; i < 8; i++)
		{
			tmp = (char)(crc & 0x0001);		// Sacuvaj test b0
			crc >>= 1;
			if (tmp != 0) crc ^= 40961;	// Ako je b0 bio 1 uradi operaciju. ModBus generator je 40961
		}
	}

	/// <summary>
	/// CRC koji je dobijen od podataka u Data pakuje u 4 karaktera koja predstavljaju dva bajta (16b)crc-a.
	/// Dobijeni string vraca da bi se kasnije dodao u paket koji se salje
	/// </summary>
	/// <param name="Data"></param>
	/// <returns></returns>
	string PackCS(string &Data)
	{
		// Izracunaj CS
//		char csbuff[25] = Data.c_str();
		ushort crc = 65535;
//		for (int i = 0; i < csbuff.Length; i++)
//		for (int i = 0; i < 8; i++)
		for (int i = 0; i < 8; i++)
		{
//			CRC16(Data.c_str(), crc);
		}

		string outstring;
		outstring += (char)((crc >> 4) & 0x000F);   // Hi nibl HiB
		outstring += (char)(crc & 0x000F);   // Hi nibl HiB
		outstring += (char)((crc >> 12) & 0x000F);   // Hi nibl LoB
		outstring += (char)((crc >> 8) & 0x000F);   // Lo nibl LoB

		return outstring;
	}


//	void Go(byte messageId, Enums.PowerLevel leftPower, Enums.PowerLevel rightPower, Enums.MotorDirection leftDirection, Enums.MotorDirection rightDirection)
	void go(int left, int right)
	{
		leftPower=abs(left);
		rightPower=abs(right);
		string outstr;
//		outstr += (char)_startAddress;  // Start bit + device address
//		outstr += (char)_myAddress;     // Moja adresa, najcesce je to 0x01 default master ali moze biti mnogo drugih radi testa
		outstr += (char)0x82;  // Start bit + device address
		outstr += (char)0x01;
		outstr += (char)12;      // Broj bajtova
		outstr += (char)0x04;   // Komanda Go Hi nible
		outstr += (char)0x0F;   // Komanda Go Lo nible
		outstr += (char)(messageId >> 4);    // Ostavi samo gornji nibl, ali ga prebaci u nizi transportnog bajta
		outstr += (char)(messageId & 0x0F);  // Donji nibl snage levog motora
		outstr += (char)((char)(leftPower) >> 4);    // Ostavi samo gornji nibl, ali ga prebaci u nizi transportnog bajta
		outstr += (char)((char)(leftPower) & 0x0F);  // Donji nibl snage levog motora
		outstr += (char)((char)(rightPower) >> 4);   // Gornji nibl snage desnog motora
		outstr += (char)((char)(rightPower) & 0x0F); // Donji nibl snage desnog motora

//		if (leftDirection == Enums.MotorDirection.CW)
		if (left>0)
		{
			outstr += (char)0x00;
			outstr += (char)0x00;   // false
		}
		else
		{
			outstr += (char)0x00;   // Sve sto nije 0 je true
			outstr += (char)0x01;
		}

//		if (rightDirection == Enums.MotorDirection.CW)
		if (right>0)
		{
			outstr += (char)0x00;
			outstr += (char)0x00;   // false
		}
		else
		{
			outstr += (char)0x00;   // Sve sto nije 0 je true
			outstr += (char)0x01;
		}

//		outstr += PackCS(outstr.substr(3,20)));

		//return outstr;
		serial<<outstr;
	}


//	void go(int left, int right)
//	{
//		char buffer[13];
//		leftPower=abs(left);
//		rightPower=abs(right);
//		serial<<(uchar)0x82;  // Start bit + device address
//        serial<<(uchar)0x01;     // Moja adresa, najcesce je to 0x01 default master ali moze biti mnogo drugih radi testa
//        serial<<(uchar)12;      // Broj bajtova
//        serial<<(uchar)0x04;   // Komanda Go Hi nible
//        buffer[0]=(uchar)0x04;
//        serial<<(uchar)0x0F;   // Komanda Go Lo nible
//        buffer[1]=(uchar)0x0F;
//        serial<<(uchar)(messageId >> 4);// Ostavi samo gornji nibl, ali ga prebaci u nizi transportnog bajta
//        buffer[2]=(uchar)(messageId >> 4);
//        serial<<(uchar)(messageId & 0x0F);  // Donji nibl snage levog motora
//        buffer[3]=(uchar)(messageId & 0x0F);
//        serial<<(uchar)((leftPower) >> 4);    // Ostavi samo gornji nibl, ali ga prebaci u nizi transportnog bajta
//        buffer[4]=(uchar)((leftPower) >> 4);
//        serial<<(uchar)((leftPower) & 0x0F);  // Donji nibl snage levog motora
//        buffer[5]=(uchar)((leftPower) & 0x0F);
//        serial<<(uchar)((rightPower) >> 4);   // Gornji nibl snage desnog motora
//        buffer[6]=(uchar)((rightPower) >> 4);
//        serial<<(uchar)((rightPower) & 0x0F); // Donji nibl snage desnog motora
//        buffer[7]=(uchar)((rightPower) & 0x0F);
//
//        if (left>0)
//        {
//        	serial<<(uchar)0x00;
//            serial<<(uchar)0x00;   // false
//            buffer[8]=(uchar)0x00;
//            buffer[9]=(uchar)0x00;
//        }
//        else
//        {
//			serial<<(uchar)0x00;   // Sve sto nije 0 je true
//			serial<<(uchar)0x01;
//			buffer[8]=(uchar)0x00;
//			buffer[9]=(uchar)0x01;
//        }
//
//        if (right<0)
//        {
//        	serial<<(uchar)0x00;
//        	serial<<(uchar)0x00;   // false
//        	buffer[10]=(uchar)0x00;
//        	buffer[11]=(uchar)0x00;
//        }
//        else
//        {
//        	serial<<(uchar)0x00;   // Sve sto nije 0 je true
//        	serial<<(uchar)0x01;
//        	buffer[10]=(uchar)0x00;
//        	buffer[11]=(uchar)0x01;
//        }
//        serial<<PackCS(buffer[0]);
//
//	}
//
//	void CRC16(char& Data, ushort &crc)
//	{ // Racuna CRC
//		char tmp;	// dovoljan je byte jer uvek testiram b0
//		crc ^= Data;
//		for (int i = 0; i < 8; i++)
//		{
//			tmp = (char)(crc & 0x0001);		// Sacuvaj test b0
//			crc >>= 1;
//			if (tmp != 0) crc ^= 40961;	// Ako je b0 bio 1 uradi operaciju. ModBus generator je 40961
//		}
//	}
//
//	string PackCS(char& Data)
//	{
//		// Izracunaj CS
//		unsigned crc = 65535;
//		for (int i = 0; i < 11; i++)
//		{
//			CRC16(*Data[i], crc);
//		}
//
//		string outstring = string.Empty;
//		outstring += (char)((crc >> 4) & 0x000F);   // Hi nibl HiB
//		outstring += (char)(crc & 0x000F);   // Hi nibl HiB
//		outstring += (char)((crc >> 12) & 0x000F);   // Hi nibl LoB
//		outstring += (char)((crc >> 8) & 0x000F);   // Lo nibl LoB
//		return outstring;
//	}


	int left(int speed)
	{
		if(speed > max)
			speed=max;
		else if(speed < -max)
			speed=-max;
		else if(speed>=1 && speed<min)
			speed=min;
		else if(speed<=-1 && speed>-min)
			speed=-min;
		if (speed<0)
		{
			speed*=-1;
			speed+=128;
		}
		else
			speed+=192;
		serial<<(uchar)speed;
		return (int) speed;
	}

	int right(int speed)
	{
		if(speed > max)
			speed=max;
		else if(speed < -max)
			speed=-max;
		else if(speed>=1 && speed<min)
			speed=min;
		else if(speed<=-1 && speed>-min)
			speed=-min;
		if (speed<0)
		{
			speed*=-1;
			speed+=64;
		}
		serial<<(uchar)speed;
		return (int) speed;
	}
};

#endif /* ENGINES_H_ */


